#include "simulation/map.h"

#include <nav_msgs/OccupancyGrid.h>

#include <iostream>
#include <fstream>
#include <vector>

#include "math.h"

#define MAP_PIXEL_SIZE 0.01f

#define MAP_EMPTY 0
#define MAP_WALL 1

Map::Map(std::string filename, int width, int height) :
	map_width(width),
	map_height(height),
	x_offset(-0.20f),
	y_offset(1.81f)
{
	ros::NodeHandle nh;
	map_publisher = nh.advertise<nav_msgs::OccupancyGrid>("virtual_map", 1);

	// Allocate map array
	map = (char **) malloc(sizeof(char *) * map_width);
	for (int i = 0; i < map_width; i++) {
		map[i] = (char *) malloc(sizeof(char) * map_height);
	}

	read_map_from_file(filename);
}

/**
 * Opens and reads the map file at file_path, entering the read data
 * into the map matrix.
 */
void Map::read_map_from_file(std::string file_path)
{
	std::ifstream file(file_path.c_str(), std::ios::in | std::ios::binary);

	if(file.is_open())
	{
		ROS_INFO("Opened map file at %s\n", file_path.c_str());

		int bytes_read = 0;
		for (int y = 0; y < map_height; y++) {
			for (int x = 0; x < map_width; x++) {
				int r, g, b;
				r = file.get();
				g = file.get();
				b = file.get();

				map[x][y] = (r + g + b == 0 ? MAP_WALL : MAP_EMPTY);
				bytes_read += 3;
			}
		}

		ROS_INFO("Read %d bytes from file\n", bytes_read);

		file.close();
	} else {
		ROS_ERROR("Could not open map file at %s\n", file_path.c_str());
	}
}

/**
 * Returns true if there is no wall at this coordinate, false otherwise.
 */
bool Map::is_empty(float x, float y)
{
	int idx_x = x / MAP_PIXEL_SIZE;
	int idx_y = map_height - y / MAP_PIXEL_SIZE;

	if (idx_x < 0 || idx_x >= map_width ||
		idx_y < 0 || idx_y >= map_height) {
		return true;
	} else {
		return map[idx_x][idx_y] == MAP_EMPTY;
	}
}

/**
 * Returns the distance from coordinate {x, y} to the nearest wall in the
 * direction specified. If there's no wall within max_range, 0 is returned.
 */
float Map::distance(float x, float y, float direction, float max_range)
{
	// Offset map
	x += x_offset;
	y += y_offset;

	// Calculate end position
	float end_x = x + max_range * cosf(direction);
	float end_y = y + max_range * sinf(direction);

	float curr_x, curr_y;

	float t = 0.0f;
	while (t <= 1.0f) {
		curr_x = (1.0f - t) * x + t * end_x;
		curr_y = (1.0f - t) * y + t * end_y;

		if (!is_empty(curr_x, curr_y)) {
			float dx = curr_x - x;
			float dy = curr_y - y;

			return sqrtf(dx * dx + dy * dy);
		}

		t += 0.01f;
	}

	// No wall found, return 0 distance
	return 0.0f;
}

void Map::render()
{
	nav_msgs::OccupancyGrid occupancyGrid;

	occupancyGrid.header.frame_id = "/floor";
	occupancyGrid.header.stamp = ros::Time::now();

	occupancyGrid.info.map_load_time = ros::Time::now();
	occupancyGrid.info.resolution = MAP_PIXEL_SIZE;
	occupancyGrid.info.width = map_width;
	occupancyGrid.info.height = map_height;

	occupancyGrid.info.origin.position.x = -x_offset;
	occupancyGrid.info.origin.position.y = -y_offset;
	occupancyGrid.info.origin.position.z = 0.0f;

	occupancyGrid.info.origin.orientation.x = 0.0f;
	occupancyGrid.info.origin.orientation.y = 0.0f;
	occupancyGrid.info.origin.orientation.z = 0.0f;
	occupancyGrid.info.origin.orientation.w = 1.0f;

	std::vector<int8_t> map_data(map_width * map_height);
	int count = 0;
	for (int y = 0; y < map_height; y++) {
		for (int x = 0; x < map_width; x++) {
			map_data[count++] = map[x][map_height - y - 1];
		}
	}

	occupancyGrid.data = map_data;

	map_publisher.publish(occupancyGrid);
}

